"""
这个代码模拟了一个智能防抖勺子的工作原理：
1、AntiShakeSpoon 类：实现了防抖勺子的核心功能，包括：
 · 初始化参数（滤波强度和历史数据大小）
 · 启动和停止控制系统
 · 防抖算法的实现（基于历史数据的加权平均）
 · 更新手部位置和获取稳定后的勺子位置
2、防抖算法：
 · 使用历史位置数据的加权平均值来平滑手部抖动
 · 滤波强度参数控制平滑程度，值越大防抖效果越明显
3、模拟使用：
 · 生成模拟的手部移动轨迹
 · 添加随机抖动模拟手部不稳定
 · 展示防抖前后的位置对比
4、在实际应用中，这个系统需要配合传感器（如加速度计、陀螺仪）和电机控制系统来实现物理防抖功能。代码中的防抖算法可以根据具体需求进行调整和优化。
"""

import time
import random
import threading
from collections import deque




class AntiShakeSpoon:
    def __int__(self, filter_strength=0.7,history_size=10):
        """
        初始化智能防抖勺子

        参数：
        - filter_strength: 滤波强度，值越大防抖效果越明显
        - history_size: 历史数据队列大小
        """
        self.filter_strength = filter_strength  # 滤波强度，值越大防抖效果越明显
        self.history = deque(maxlen=history_size) # 历史数据大小，最大抖动范围
        self.current_position = (0, 0, 0) # 初始化位置(x, y, z)
        self.target_position = (0, 0, 0)  # 目标位置
        self.is_running = False # 开始运行
        self.control_thread = None # 控制
        self.lock = threading.Lock()   #勺子

        def start(self):
            """启动防抖系统"""
            if not self.is_running:
                self.is_running = True
                self.control_thread = threading.Thread(target=self._control_loop)
                self.control_thread.daemon = True
                self.control_thread.start()
                print("智能防抖勺子已启动")

        def stop(self):
            """停止防抖系统"""
            self.is_running = False
            if self.control_thread:
               self.control_thread.jion(timeout=1.0)
            print("智能防抖勺子已停止")

        def _control_loop(self):
            """控制系统主循环"""
            while self.is_running:
                # 应用防抖算法计算稳定位置
                stable_position = self._apply_anti_shake()

                # 更新勺子位置（这里只是模拟，实际应用中会控制电机）
                with self.lock:
                    self.current_position = stable_position

                # 模拟控制频率
                time.sleep(0.01)

        def _apply_anti_shake(self):
            """应用防抖算法"""
            if not self.history:
                return self.target_position

            # 计算历史数据的加权平均值
            weights = [i + 1 for i  in range(len(self.history))]
            sum_weights = sum(weights)

            weights_x = sum(pos[0] * w for pos, w in zip(self.history, weights)) / sum_weights
            weights_y = sum(pos[1] * w for pos, w in zip(self.history, weights)) / sum_weights
            weights_z = sum(pos[2] * w for pos, w in zip(self.history, weights)) / sum_weights

            # 结合当前目标位置和历史平均值进行平滑
            stable_x = (1 - self.filter_strength) * self.target_position[0] + self.filter_strength * weights_x
            stable_y = (1 - self.filter_strength) * self.target_position[1] + self.filter_strength * weights_y
            stable_z = (1 - self.filter_strength) * self.target_position[2] + self.filter_strength * weights_z

            return (stable_x,stable_y, stable_z)
        def update_hand_position(self, x, y, z):
            """更新手部位置（模拟传感器输入）"""
            # 添加随机抖动模拟手部不稳定
            shake_x = random.uniform(-0.5, 0.5)
            shake_y = random.uniform(-0.5, 0.5)
            shake_z = random.uniform(-0.5,0.5)

            # 带有抖动的手部位置
            hand_position = (x + shake_x, y + shake_y, z + shake_z)

            with self.lock:
                self.target_position = hand_position
                self.history.append(hand_position)

        def get_spoon_position(self):
            """获取勺子当前稳定位置"""
            with self.lock:
                return self.current_position

    # 使用示例
def simulate_spoon_usage():
    spoon = AntiShakeSpoon(filter_strength=0.8)
    spoon.start()

    try:
        print("模拟智能防抖勺子使用...")
        # 模拟手部移动轨迹
        path  = [(i*0.1, i*0.05, 0.2) for i in range(50)]

        for pos in path:
          # 更新手部位置
          spoon.update_hand_position(*pos)

          # 获取稳定后的勺子位置
          stable_pos = spoon.get_spoon_position()

          # 打印当前状态
          print(f"手部位置：({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})"
                f" -> 勺子位置：({stable_pos[0]:.2f}, {stable_pos[1]:.2f}, {stable_pos[2]:.2f})")

          time.sleep(0.1)

    finally:
        spoon.stop()



if __name__ == "__main__":
    simulate_spoon_usage()








